    #include "paplan.h"

Paplan::Paplan(QObject *parent) : QObject(parent)
{
    vmap = imread("E:/QT_corder/SmartCar_fundenment/chepter5/PathPlanning/P1000C1.bmp");
    //imshow("map",vmap);
    Mat vvmap = vmap.clone();
    Mat a;
    Mat b;
    //imshow("map",vvmap);
    //qDebug() << vvmap.rows;

    //getmap函数在此实现
    int s = 40*2+1;
    Mat element = getStructuringElement(MORPH_RECT, Size(s, s), Point(-1, -1));      //创建结构元

    erode(vvmap, dispimagine, element);
    resize(vvmap,a,Size(600,600));
    resize(dispimagine,b,Size(600,600));
    imshow("After Erode",b);
    imshow("Before Erode",a);
    map.resize(vvmap.rows);
    for (int i =0; i < map.size();i++)
    {
        map[i].resize(vvmap.cols);
        for(int j=0;j<map[i].size();j++)
        {
            if (dispimagine.at<Vec3b>(i,j)!=Vec3b(255,255,255))
            {
                map[i][j]=1;
            }
            else
            {
                map[i][j]=0;
            }
        }
    }
    result = Astar(map, 100, 900, 800, 100, m_x, m_y);
    if(result)
    {
        qDebug()  << "Find Path:";
        x.resize(m_x.size());
        y.resize(m_y.size());
        for (int i = 0; i < m_x.size()-1; i++)
        {
            x[i] = m_x[i];
            y[i] = m_y[i];

//            qDebug()  << m_x[i] << " " << m_y[i] ;
            qDebug()  << x[i] << " " << y[i] ;
        }
    }

}
